/*
 * This file is part of the Sx Framework Library.
 * 
 * Copyright (C) 2013 University of Colorado Denver
 * <min.choi@ucdenver.edu> <shane.transue@ucdenver.edu>
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy 
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 
 * copies of the Software, and to permit persons to whom the Software is 
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in 
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 
 * DEALINGS IN THE SOFTWARE.
 */
#include <sxMSM_TS_InverseVelocityResponseHandler.h>
#include <sxIntersections.h>
#include <sxDistances.h>
#include <set>

Sx::Simulation::Collision::Handlers::Response::MSM_TS_InverseVelocityResponseHandler::MSM_TS_InverseVelocityResponseHandler() {}
Sx::Simulation::Collision::Handlers::Response::MSM_TS_InverseVelocityResponseHandler::~MSM_TS_InverseVelocityResponseHandler() {}

bool Sx::Simulation::Collision::Handlers::Response::MSM_TS_InverseVelocityResponseHandler::construct(Util::ArrayList<std::shared_ptr<Objects::MassSpringMesh> >& objects) {
	return true;
}

bool Sx::Simulation::Collision::Handlers::Response::MSM_TS_InverseVelocityResponseHandler::update(Util::ArrayList<std::shared_ptr<Objects::MassSpringMesh> >& objects, Physics::Collision::Aggregates::TriangleSoupCollisionEventAggregate<Objects::MassSpringMesh>* const collisionData) {
	return true;
}

/* Based on: http://geomalgorithms.com/a07-_distance.html */
Eigen::Vector3d ClosestMidpointBetweenTwoSegments(const Eigen::Vector3d& segment_1_p0, const Eigen::Vector3d& segment_1_p1, const Eigen::Vector3d& segment_2_p0, const Eigen::Vector3d& segment_2_p1, Eigen::Vector3d& segment_1_closestPoint, Eigen::Vector3d& segment_2_closestPoint) {
	Eigen::Vector3d u = segment_1_p1 - segment_1_p0;
    Eigen::Vector3d v = segment_2_p1 - segment_2_p0;
    Eigen::Vector3d w = segment_1_p0 - segment_2_p0;

    double a = u.dot(u);
    double b = u.dot(v);
    double c = v.dot(v);
    double d = u.dot(w);
    double e = v.dot(w);

    double D = a*c - b*b;
    double sc, sN, sD = D;
    double tc, tN, tD = D;

    if ( D < std::numeric_limits<double>::epsilon() ) {
        sN = 0.0;
        sD = 1.0;
        tN = e;
        tD = c;
    }
    else {
        sN = (b*e - c*d);
        tN = (a*e - b*d);
        if (sN < 0.0) {
            sN = 0.0;
            tN = e;
            tD = c;
        }
        else if (sN > sD) {
            sN = sD;
            tN = e + b;
            tD = c;
        }
    }

    if (tN < 0.0) {
        tN = 0.0;

        if (-d < 0.0)
            sN = 0.0;
        else if (-d > a)
            sN = sD;
        else {
            sN = -d;
            sD = a;
        }
    }
    else if (tN > tD) {
        tN = tD;

        if ((-d + b) < 0.0)
            sN = 0;
        else if ((-d + b) > a)
            sN = sD;
        else {
            sN = (-d + b);
            sD = a;
        }
    }

    sc = (abs(sN) < std::numeric_limits<double>::epsilon() ? 0.0 : sN / sD);
    tc = (abs(tN) < std::numeric_limits<double>::epsilon() ? 0.0 : tN / tD);

	segment_1_closestPoint = segment_1_p0 + (u * sc);
	segment_2_closestPoint = segment_2_p0 + (v * tc);

	return Eigen::Vector3d((segment_1_closestPoint.x() + segment_2_closestPoint.x()) / 2.0, (segment_1_closestPoint.y() + segment_2_closestPoint.y()) / 2.0, (segment_1_closestPoint.z() + segment_2_closestPoint.z()) / 2.0);
}

Eigen::Vector3d GetReflectionNormal(const Eigen::Vector3d& endPoint0, const Eigen::Vector3d& endPoint1, const Eigen::Vector3d& velocity) {
	Eigen::Vector3d vDir = velocity.normalized();
	Eigen::Vector3d ep0ep1 = (endPoint1 - endPoint0).normalized();
	Eigen::Vector3d temp = ep0ep1.cross(vDir).normalized();
	Eigen::Vector3d n = ep0ep1.cross(temp).normalized();
	return n;
}

Eigen::Vector3d GetReflectedVelocity(const Eigen::Vector3d& ta, const Eigen::Vector3d& tb, const Eigen::Vector3d& tc, const Eigen::Vector3d& velocity) {
	Eigen::Vector3d n = Sx::Math::Normal(ta, tb, tc);
	Eigen::Vector3d vDir = velocity.normalized();

	double nDotv = n.dot(vDir);
	Eigen::Vector3d a = n * nDotv;
	if ( vDir.dot(n) < 0.0 ) a *= -1.0;
	Eigen::Vector3d twoA = (2.0 * a);
	Eigen::Vector3d reflectionDir = (vDir + twoA).normalized();
	double velMag = velocity.norm();
	Eigen::Vector3d reflectedVelocity = reflectionDir * velMag;
	return reflectedVelocity;
}

Eigen::Vector3d GetReflectedVelocity(const Eigen::Vector3d& endPoint0, const Eigen::Vector3d& endPoint1, const Eigen::Vector3d& velocity) {
	Eigen::Vector3d vDir = velocity.normalized();
	Eigen::Vector3d ep0ep1 = (endPoint1 - endPoint0).normalized();
	Eigen::Vector3d temp = ep0ep1.cross(vDir).normalized();
	Eigen::Vector3d n = ep0ep1.cross(temp).normalized();

	double nDotv = n.dot(vDir);
	Eigen::Vector3d a = n * nDotv;
	if ( vDir.dot(n) < 0.0 ) a *= -1.0;
	Eigen::Vector3d twoA = (2.0 * a);
	Eigen::Vector3d reflectionDir = (vDir + twoA).normalized();
	double velMag = velocity.norm();
	Eigen::Vector3d reflectedVelocity = reflectionDir * velMag;
	return reflectedVelocity;
}

bool PositionMoved(const Eigen::Vector3d& originalPosition, const Eigen::Vector3d& newPosition, double threshold = 0.0001) {
	double xDiff = std::abs(newPosition.x() - originalPosition.x());
	double yDiff = std::abs(newPosition.y() - originalPosition.y());
	double zDiff = std::abs(newPosition.z() - originalPosition.z());
	if ( xDiff > threshold ) return true;
	if ( yDiff > threshold ) return true;
	if ( zDiff > threshold ) return true;
	return false;
}

bool GetInterseciontPoint(const Eigen::Vector3d& t0, const Eigen::Vector3d& t1, const Eigen::Vector3d t2, const Eigen::Vector3d& prev, const Eigen::Vector3d& next, Eigen::Vector3d& negInf, Eigen::Vector3d& inf, Eigen::Vector3d& point) {
	Eigen::Vector3d negDir = (next - prev).normalized();
	Eigen::Vector3d posDir = (prev - next).normalized();
	Eigen::Vector3d toss;
	Sx::Math::LineFromParametric(prev, negDir, 1000.0f, toss, negInf);
	Sx::Math::LineFromParametric(next, posDir, 1000.0f, toss, inf);
	return  Sx::Math::Intersections::SegmentIntersectTriangled(negInf, inf, t0, t1, t2, point);
}

/* object has the moving vertex, other has the static triangle. */
bool Handle_Vertex_Moving_Triangle_Static(const std::shared_ptr<Sx::Simulation::Objects::MassSpringMesh>& object, const std::shared_ptr<Sx::Simulation::Objects::MassSpringMesh>& other, unsigned int vIndex, unsigned int tIndex0, unsigned int tIndex1, unsigned int tIndex2) {
	if ( object == nullptr || object.use_count() == 0 ) return false;
	if ( other == nullptr || other.use_count() == 0 ) return false;
	
	Eigen::Vector3d vertex, pVertex, velocity, t0, t1, t2, intersectionPoint, invertedIntersectionDir, correctionDisplacement, correctedPosition;
	Eigen::Vector3d infTestSegment0, infTestSegment1;

	//--------------------------------------------------------------------------
	// Get data from the colliding objects.
	//--------------------------------------------------------------------------
	object->getObject()->transformPointToGlobal(object->getNodePosition(vIndex), vertex);
	object->getObject()->transformPointToGlobal(object->getNodePrevPosition(vIndex), pVertex);
	velocity = object->getNodeVelocity(vIndex);
	other->getObject()->transformPointToGlobal(other->getNodePosition(tIndex0), t0);
	other->getObject()->transformPointToGlobal(other->getNodePosition(tIndex1), t1);
	other->getObject()->transformPointToGlobal(other->getNodePosition(tIndex2), t2);

	//--------------------------------------------------------------------------
	// Get the intersection point between a line formed by (pVertex and vertex)
	// and the triangle. This will be used to calculate the penetration depth.
	//--------------------------------------------------------------------------
	bool intersectedCurrent = GetInterseciontPoint(t0, t1, t2, pVertex, vertex, infTestSegment0, infTestSegment1,  intersectionPoint);
	if ( intersectedCurrent == false ) {
		std::cerr << "[Handle_Vertex_Moving_Triangle_Static] Error: Inf. Line-Triangle test returned false." << std::endl;
		std::cerr << "  This should not be possible for the reported collision." << std::endl;
		return false;
	}

	double CORRECTION_OFFSET_PERCENTAGE = 0.6; // 1.0 works but its a little high..
	invertedIntersectionDir = (intersectionPoint - vertex).normalized();
	double penetrationDist = Sx::Math::Distance(intersectionPoint, vertex);
	penetrationDist += (penetrationDist * CORRECTION_OFFSET_PERCENTAGE);

	//--------------------------------------------------------------------------
	// Calculate correction displacement and new position.
	//--------------------------------------------------------------------------
	correctionDisplacement = invertedIntersectionDir * penetrationDist;
	correctedPosition = vertex + correctionDisplacement;

	//--------------------------------------------------------------------------
	// Inverse the velocity and apply damping.
	//--------------------------------------------------------------------------
	//velocity *= -1.0;
	//velocity -= (velocity * 0.3);
	//--------------------------------------------------------------------------
	Eigen::Vector3d triangleNormal = Sx::Math::Normal(t0, t1, t2);
	double mag = velocity.norm();
	Eigen::Vector3d newVelocity = triangleNormal * mag;
	//Eigen::Vector3d newVelocity = GetReflectedVelocity(t0, t1, t2, velocity); // this has a problem..
	//if ( testVelocity.dot(triangleNormal) < 0 )
	//	std::cout << "The reflected velocity is wrong." << std::endl;
	// add a little in the direction of the triangle normal to help it slide..
	
	
	// sliding
	Eigen::Vector3d vDir = velocity.normalized();
	Eigen::Vector3d temp = vDir.cross(triangleNormal).normalized();
	Eigen::Vector3d slideDir = temp.cross(triangleNormal).normalized();
	double slideMag = slideDir.dot(velocity * 0.3);

	Eigen::Vector3d slideVel = slideDir * slideMag;
	newVelocity += slideVel;

	newVelocity += (triangleNormal * slideVel.norm());

	newVelocity -= (newVelocity * 0.4);

	object->setNodeVelocity(vIndex, newVelocity);
	object->addNodePosition(vIndex, correctionDisplacement);

	//--------------------------------------------------------------------------
	// Simplest solution
	//velocity *= -1.0;
	//velocity -= (velocity * 0.3);
	//object->setNodeVelocity(vIndex, velocity);
	//object->setNodePosition(vIndex, pVertex);
	//--------------------------------------------------------------------------
	return true;
}

/* object has the static vertex, other has the moving triangle. */
bool Handle_Triangle_Moving_Vertex_Static(const std::shared_ptr<Sx::Simulation::Objects::MassSpringMesh>& object, const std::shared_ptr<Sx::Simulation::Objects::MassSpringMesh>& other, unsigned int vIndex, unsigned int tIndex0, unsigned int tIndex1, unsigned int tIndex2) {
	if ( object == nullptr || object.use_count() == 0 ) return false;
	if ( other == nullptr || other.use_count() == 0 ) return false;

	Eigen::Vector3d vertex, t0, t1, t2, tVel0, tVel1, tVel2, intersectionPoint, invertedIntersectionDir, triangleNormal, correctionDisplacement;
	Eigen::Vector3d correctedPositionT0, correctedPositionT1, correctedPositionT2;

	vertex = object->getNodePosition(vIndex);
	t0 = other->getNodePosition(tIndex0);
	t1 = other->getNodePosition(tIndex1);
	t2 = other->getNodePosition(tIndex2);
	tVel0 = other->getNodeVelocity(tIndex0);
	tVel1 = other->getNodeVelocity(tIndex1);
	tVel2 = other->getNodeVelocity(tIndex2);
	// Determine the penetration depth by finding the distance between the point
	// and the moving triangle.
	triangleNormal = Sx::Math::Normal(t0, t1, t2);
	double CORRECTION_OFFSET_PERCENTAGE = 0.10;
	double penetrationDist = Sx::Math::Distances::PointToPlaneDistance(triangleNormal, t0, vertex, intersectionPoint);
	penetrationDist += (penetrationDist * CORRECTION_OFFSET_PERCENTAGE);

	// The direction to displace the triangle vertices.
	invertedIntersectionDir = -triangleNormal;
	//NEW
	Eigen::Vector3d correctionDir = (vertex - intersectionPoint).normalized();
	correctionDisplacement = invertedIntersectionDir * penetrationDist;
	correctedPositionT0 = t0 + correctionDisplacement;
	correctedPositionT1 = t1 + correctionDisplacement;
	correctedPositionT2 = t2 + correctionDisplacement;

	//--------------------------------------------------------------------------
	// Inverse the velocities and apply damping.
	//--------------------------------------------------------------------------
	//tVel0 *= -1.0;
	//tVel1 *= -1.0;
	//tVel2 *= -1.0;
	//tVel0 -= (tVel0 * 0.3);
	//tVel1 -= (tVel1 * 0.3);
	//tVel2 -= (tVel2 * 0.3);
	//--------------------------------------------------------------------------
	// use the average magnitude of the triangle vertices for mag and -triangle normal for dir'
	Eigen::Vector3d dir = -triangleNormal;
	Eigen::Vector3d avgVel = (tVel0 + tVel1 + tVel2) / 3.0;
	double avgMag = avgVel.norm();
	Eigen::Vector3d newVel = dir * avgMag;
	tVel0 = -avgVel;//-1.0;//newVel;
	tVel1 = -avgVel;//-1.0;//newVel;
	tVel2 = -avgVel;//-1.0;//newVel;
	//damping
	//tVel0 -= (tVel0 * 0.3);
	//tVel1 -= (tVel1 * 0.3);
	//tVel2 -= (tVel2 * 0.3);

	//SLIDE
	Eigen::Vector3d temp = -avgVel.cross(-triangleNormal).normalized();
	Eigen::Vector3d slideDir = temp.cross(-triangleNormal).normalized();
	double slideMag = avgVel.dot(slideDir) * 0.01;
	//if ( slideMag < -0.05 || slideMag > 0.0 )
	//	int as = 23;
	Eigen::Vector3d slideVel = slideDir * slideMag;
	tVel0 += slideVel;
	tVel1 += slideVel;
	tVel2 += slideVel;

	other->setNodePosition(tIndex0, correctedPositionT0);
	other->setNodePosition(tIndex1, correctedPositionT1);
	other->setNodePosition(tIndex2, correctedPositionT2);
	other->setNodeVelocity(tIndex0, tVel0);
	other->setNodeVelocity(tIndex1, tVel1);
	other->setNodeVelocity(tIndex2, tVel2);

	//--------------------------------------------------------------------------
	// Simplest solution..
	//Eigen::Vector3d t0p = other->getNodePrevPosition(tIndex0);
	//Eigen::Vector3d t1p = other->getNodePrevPosition(tIndex1);
	//Eigen::Vector3d t2p = other->getNodePrevPosition(tIndex2);
	//other->setNodePosition(tIndex0, t0p);
	//other->setNodePosition(tIndex1, t1p);
	//other->setNodePosition(tIndex2, t2p);
	//other->setNodeVelocity(tIndex0, tVel0);
	//other->setNodeVelocity(tIndex1, tVel1);
	//other->setNodeVelocity(tIndex2, tVel2);
	//--------------------------------------------------------------------------
	return true;
}

bool Handle_Triangle_Moving_Vertex_Moving(const std::shared_ptr<Sx::Simulation::Objects::MassSpringMesh>& object, const std::shared_ptr<Sx::Simulation::Objects::MassSpringMesh>& other, unsigned int vIndex, unsigned int tIndex0, unsigned int tIndex1, unsigned int tIndex2) {
	if ( object == nullptr || object.use_count() == 0 ) return false;
	if ( other == nullptr || other.use_count() == 0 ) return false;

	Eigen::Vector3d vertex, pVertex, velocity, t0, t1, t2, pt0, pt1, pt2, tVel0, tVel1, tVel2;
	Eigen::Vector3d triangleNormal, invTriangleNormal, intersectionPoint;
	
	object->getObject()->transformPointToGlobal(object->getNodePosition(vIndex), vertex);
	object->getObject()->transformPointToGlobal(object->getNodePrevPosition(vIndex), pVertex);
	velocity = object->getNodeVelocity(vIndex);

	other->getObject()->transformPointToGlobal(other->getNodePosition(tIndex0), t0);
	other->getObject()->transformPointToGlobal(other->getNodePosition(tIndex1), t1);
	other->getObject()->transformPointToGlobal(other->getNodePosition(tIndex2), t2);
	//pt0 = other->getNodePrevPosition(tIndex0);
	//pt1 = other->getNodePrevPosition(tIndex1);
	//pt2 = other->getNodePrevPosition(tIndex2);
	tVel0 = other->getNodeVelocity(tIndex0);
	tVel1 = other->getNodeVelocity(tIndex1);
	tVel2 = other->getNodeVelocity(tIndex2);

	triangleNormal = Sx::Math::Normal(t0, t1, t2);
	invTriangleNormal = -triangleNormal;
	double CORRECTION_OFFSET_PERCENTAGE = 0.30;
	double penetrationDist = Sx::Math::Distances::PointToPlaneDistance(triangleNormal, t0, vertex, intersectionPoint);
	double halPenetrationDepth = penetrationDist / 2.0;
	double adjustedPenetrationDepth = halPenetrationDepth + (halPenetrationDepth * CORRECTION_OFFSET_PERCENTAGE);

	Eigen::Vector3d vertexDispDir = triangleNormal;
	Eigen::Vector3d triangleDispDir = invTriangleNormal;

	Eigen::Vector3d newVertexDisp = vertexDispDir * adjustedPenetrationDepth;
	Eigen::Vector3d newT0Disp = triangleDispDir * adjustedPenetrationDepth;
	Eigen::Vector3d newT1Disp = triangleDispDir * adjustedPenetrationDepth;
	Eigen::Vector3d newT2Disp = triangleDispDir * adjustedPenetrationDepth;

	Eigen::Vector3d newVertexPos = vertex + newVertexDisp;
	Eigen::Vector3d newT0Pos = t0 + newT0Disp;
	Eigen::Vector3d newT1Pos = t1 + newT1Disp;
	Eigen::Vector3d newT2Pos = t2 + newT2Disp;

	velocity *= -1.0;
	velocity -= (velocity * 0.3);

	tVel0 *= -1.0;
	tVel1 *= -1.0;
	tVel2 *= -1.0;
	tVel0 -= (tVel0 * 0.3);
	tVel1 -= (tVel1 * 0.3);
	tVel2 -= (tVel2 * 0.3);

	object->addNodePosition(vIndex, newVertexDisp);
	object->setNodeVelocity(vIndex, velocity);

	other->addNodePosition(tIndex0, newT0Disp);
	other->addNodePosition(tIndex1, newT0Disp);
	other->addNodePosition(tIndex2, newT0Disp);
	other->setNodeVelocity(tIndex0, tVel0);
	other->setNodeVelocity(tIndex1, tVel1);
	other->setNodeVelocity(tIndex2, tVel2);

	return true;
}

/*
 * Edge a is moving, Edge b is static.
 */
void EndpointCausedCollision(
	const Eigen::Vector3d& a0, const Eigen::Vector3d& a1, const Eigen::Vector3d& ap0, const Eigen::Vector3d& ap1,
	const Eigen::Vector3d& b0, const Eigen::Vector3d& b1, const Eigen::Vector3d& bp0, const Eigen::Vector3d& bp1,
	const Eigen::Vector3d& a0v, const Eigen::Vector3d& a1v) {

	bool a0CausedCollision = false;
	bool a1CausedCollision = false;

	if ( a0v.norm() == 0.0 ) {
		std::cerr << "[EndpointCausedCollision] Error: The endpoint a0 has a velocity of 0." << std::endl;
		return;
	}

	if ( a1v.norm() == 0.0 ) {
		std::cerr << "[EndpointCausedCollision] Error: The endpoint a0 has a velocity of 0." << std::endl;
		return;
	}

	// Determine the closest points of the two edges.
	Eigen::Vector3d cA, cB;
	Eigen::Vector3d midpoint = ClosestMidpointBetweenTwoSegments(a0, a1, b0, b1, cA, cB);

	// Get the reflection normals from for each point (it's based on the
	// points velocity).
	Eigen::Vector3d a0_ReflectionNormal = GetReflectionNormal(b0, b1, a0v);
	Eigen::Vector3d a1_ReflectionNormal = GetReflectionNormal(b0, b1, a1v);

	Eigen::Vector3d cBToAp0Dir = (ap0 - cB).normalized();
	Eigen::Vector3d cBToA0Dir = (a0 - cB).normalized();

	Eigen::Vector3d cBToAp1Dir = (ap1 - cB).normalized();
	Eigen::Vector3d cBToA1Dir = (a1 - cB).normalized();

	bool a0_Negative = false;
	bool a0p_Negative = false;

	// The A0 endpoint has not crossed the 'plane' of B and point in the same direction as the reflection normal
	if ( cBToAp0Dir.dot(a0_ReflectionNormal) > 0 &&
		 cBToA0Dir.dot(a0_ReflectionNormal) > 0 ) {
		a0_Negative = false;
		a0p_Negative = false;
	}

	// The A0 endpoint has not crossed the 'plane' of B and point opposite of direction of the reflection normal
	if ( cBToAp0Dir.dot(a0_ReflectionNormal) < 0 &&
		 cBToA0Dir.dot(a0_ReflectionNormal) < 0 ) {
		a0_Negative = true;
		a0p_Negative = true;
	}

	// The A0 endpoint has CROSSED the 'plane' of B
	if ( cBToAp0Dir.dot(a0_ReflectionNormal) < 0 &&
		 cBToA0Dir.dot(a0_ReflectionNormal) > 0 ) {
		a0_Negative = false;
		a0p_Negative = true;
		a0CausedCollision = true; //flag
	}

	// The A0 endpoint has CROSSED the 'plane' of B
	if ( cBToAp0Dir.dot(a0_ReflectionNormal) > 0 &&
		 cBToA0Dir.dot(a0_ReflectionNormal) < 0 ) {
		a0_Negative = true;
		a0p_Negative = false;
		a0CausedCollision = true; //flag
	}





	bool a1_Negative = false;
	bool a1p_Negative = false;

	// The A1 endpoint has not crossed the 'plane' of B and point in the same direction as the reflection normal
	if ( cBToAp1Dir.dot(a1_ReflectionNormal) > 0 &&
		 cBToA1Dir.dot(a1_ReflectionNormal) > 0 ) {
		a1_Negative = false;
		a1p_Negative = false;
	}

	// The A1 endpoint has not crossed the 'plane' of B and point opposite of direction of the reflection normal
	if ( cBToAp1Dir.dot(a1_ReflectionNormal) < 0 &&
		 cBToA1Dir.dot(a1_ReflectionNormal) < 0 ) {
		a1_Negative = true;
		a1p_Negative = true;
	}

	// The A1 endpoint has CROSSED the 'plane' of B
	if ( cBToAp1Dir.dot(a1_ReflectionNormal) < 0 &&
		 cBToA1Dir.dot(a1_ReflectionNormal) > 0 ) {
		a1_Negative = false;
		a1p_Negative = true;
		a1CausedCollision = true; //flag
	}

	// The A1 endpoint has CROSSED the 'plane' of B
	if ( cBToAp1Dir.dot(a1_ReflectionNormal) > 0 &&
		 cBToA1Dir.dot(a1_ReflectionNormal) < 0 ) {
		a1_Negative = true;
		a1p_Negative = false;
		a1CausedCollision = true; //flag
	}

	bool goodA0_Collision = false;
	bool goodA1_Collision = false;
	if ( a0CausedCollision || a1CausedCollision ) {
		std::cout << "COLLISION-CAUSE: a0: " << a0CausedCollision << " a1: " << a1CausedCollision << std::endl;
		goodA0_Collision = true;
		goodA1_Collision = true;
	}
	// the points didn't span the 'B-plane' but the velocities agree 
	else {
		// POINT A0-------------------------------------------------------------
		if ( a0_Negative == true && a0p_Negative == true ) {
			goodA0_Collision = true;
		}
		else if ( a0_Negative == true && a0p_Negative == false ) {
			// this should be handled before here..
			goodA0_Collision = true;
		}
		else if ( a0_Negative == false && a0p_Negative == true ) {
			// this should be handled before here..
			goodA0_Collision = true;
		}
		else { // Both are false.
			std::cout << "?" << std::endl;
			goodA0_Collision = false;
		}

		// POINT A1-------------------------------------------------------------
		if ( a1_Negative == true && a1p_Negative == true ) {
			goodA1_Collision = true;
		}
		else if ( a1_Negative == true && a1p_Negative == false ) {
			// this should be handled before here..
			goodA1_Collision = true;
		}
		else if ( a1_Negative == false && a1p_Negative == true ) {
			// this should be handled before here..
			goodA1_Collision = true;
		}
		else { // Both are false.
			std::cout << "?" << std::endl;
			goodA1_Collision = false;
		}
	}

	std::cout << "Good_A0_Collision: " << goodA0_Collision << " Good_A1_Collision: " << goodA1_Collision << std::endl;
}

/*
Calculate the line segment PaPb that is the shortest route between
two lines P1P2 and P3P4. Calculate also the values of mua and mub where
Pa = P1 + mua (P2 - P1)
Pb = P3 + mub (P4 - P3)
Return FALSE if no solution exists.
*/
double GLH_EPSILON = 0.00001;
inline bool LineLineIntersect(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2, const Eigen::Vector3d& p3, const Eigen::Vector3d& p4, Eigen::Vector3d& pa, Eigen::Vector3d& pb, double& mua, double& mub)
{
	Eigen::Vector3d p13, p43, p21;
	float d1343, d4321, d1321, d4343, d2121;
	float numer,denom;

	p13 = p1 - p3;
	p43 = p4 - p3;
	if (fabs(p43[0])  < GLH_EPSILON && fabs(p43[1])  < GLH_EPSILON && fabs(p43[2])  < GLH_EPSILON)
		return false;

	p21 = p2 - p1;
	if (fabs(p21[0])  < GLH_EPSILON && fabs(p21[1])  < GLH_EPSILON && fabs(p21[2])  < GLH_EPSILON)
		return false;

	d1343 = p13.dot(p43);
	d4321 = p43.dot(p21);
	d1321 = p13.dot(p21);
	d4343 = p43.dot(p43);
	d2121 = p21.dot(p21);

	denom = d2121 * d4343 - d4321 * d4321;
	if (fabs(denom) < GLH_EPSILON)
		return false;
	numer = d1343 * d4321 - d1321 * d4343;

	mua = numer / denom;
	if (mua < 0 || mua > 1)
		return false;

	mub = (d1343 + d4321 * mua) / d4343;
	if (mub < 0 || mub > 1)
		return false;

	pa = p1 + p21*mua;
	pb = p3 + p43*mub;
	return true;
}

bool Handle_Edge_Edge(const std::shared_ptr<Sx::Simulation::Objects::MassSpringMesh>& object, const std::shared_ptr<Sx::Simulation::Objects::MassSpringMesh>& other, unsigned int eAIndex0, unsigned int eAIndex1, unsigned int eBIndex0, unsigned int eBIndex1, double t) {
	if ( object == nullptr || object.use_count() == 0 ) return false;
	if ( other == nullptr || other.use_count() == 0 ) return false;

	Eigen::Vector3d eA0 = object->getNodePosition(eAIndex0);
	Eigen::Vector3d eA1 = object->getNodePosition(eAIndex1);
	Eigen::Vector3d eA0v = object->getNodeVelocity(eAIndex0);
	Eigen::Vector3d eA1v = object->getNodeVelocity(eAIndex1);

	Eigen::Vector3d eB0 = other->getNodePosition(eBIndex0);
	Eigen::Vector3d eB1 = other->getNodePosition(eBIndex1);
	Eigen::Vector3d eB0v = other->getNodeVelocity(eBIndex0);
	Eigen::Vector3d eB1v = other->getNodeVelocity(eBIndex1);

	Eigen::Vector3d eAp0 = object->getNodePrevPosition(eAIndex0);
	Eigen::Vector3d eAp1 = object->getNodePrevPosition(eAIndex1);
	Eigen::Vector3d eBp0 = other->getNodePrevPosition(eBIndex0);
	Eigen::Vector3d eBp1 = other->getNodePrevPosition(eBIndex1);

	Eigen::Vector3d edgeA_AverageVelocity = (eA0v + eA1v) / 2.0;
	Eigen::Vector3d edgeB_AverageVelocity = (eB0v + eB1v) / 2.0;
	Eigen::Vector3d edgeA_ClosestPoint, edgeB_ClosestPoint;
	//Eigen::Vector3d midpoint = ClosestMidpointBetweenTwoSegments(eA0, eA1, eB0, eB1, edgeA_ClosestPoint, edgeB_ClosestPoint);
	double mua, mub;
	LineLineIntersect(eA0, eA1, eB0, eB1, edgeA_ClosestPoint, edgeB_ClosestPoint, mua, mub);

	Eigen::Vector3d eA0pv = object->getNodePrevVelocity(eAIndex0);
	Eigen::Vector3d eA1pv = object->getNodePrevVelocity(eAIndex1);
	
	if ( object->isStatic() == false && other->isStatic() == true ) {
		
		//Eigen::Vector3d correctionDir = (edgeB_ClosestPoint - edgeA_ClosestPoint).normalized();
		double penetrationDistance = Sx::Math::Distance(edgeA_ClosestPoint, edgeB_ClosestPoint);
		double CORRECTION_OFFSET_PERCENTAGE = 0.0;
		double adjustedCorrectionDist = penetrationDistance + (penetrationDistance * CORRECTION_OFFSET_PERCENTAGE);
		Eigen::Vector3d correctionDir = GetReflectionNormal(eB0, eB1, edgeA_AverageVelocity);
		Eigen::Vector3d correctionDir2 = (edgeB_ClosestPoint - edgeA_ClosestPoint).normalized();

		//if ( eAIndex0 == 3 || eAIndex0 == 7 || eAIndex1 == 3 || eAIndex0 == 7 ) {
			//int a = 0;
		//	static int i = 0;
		//	std::cout << "i: " << i << " " << correctionDir.y() << std::endl;
			//if ( midpoint.y() < 10.0 )
		//		EndpointCausedCollision(eA0, eA1, eAp0, eAp1, eB0, eB1, eBp0, eBp1, eA0v, eA1v);

		//	i++; 
		//}
		Eigen::Vector3d newDisp2 = correctionDir2 * adjustedCorrectionDist;
		Eigen::Vector3d eA_NewPos02 = eA0 + newDisp2;
		Eigen::Vector3d eA_NewPos12 = eA1 + newDisp2;

		Eigen::Vector3d newDisp = correctionDir2 * adjustedCorrectionDist;
		Eigen::Vector3d eA_NewPos0 = eA0 + newDisp;
		Eigen::Vector3d eA_NewPos1 = eA1 + newDisp;

		//object->setNodePosition(eAIndex0, eA_NewPos0);
		//object->setNodePosition(eAIndex1, eA_NewPos1);
		object->setNodePosition(eAIndex0, eAp0);
		object->setNodePosition(eAIndex1, eAp1);

		Eigen::Vector3d reflected_AVelocity = GetReflectedVelocity(eB0, eB1, edgeA_AverageVelocity);
		reflected_AVelocity -= (reflected_AVelocity * 0.2);
		//object->setNodeVelocity(eAIndex0, reflected_AVelocity);
		//object->setNodeVelocity(eAIndex1, reflected_AVelocity);
		eA0v *= -1.0f;
		eA1v *= -1.0f;
		eA0v -= (eA0v * 0.30);
		eA1v -= (eA1v * 0.30);
		object->setNodeVelocity(eAIndex0, eA0v);
		object->setNodeVelocity(eAIndex1, eA1v);
	}

	return true;
}

bool Sx::Simulation::Collision::Handlers::Response::MSM_TS_InverseVelocityResponseHandler::performResponse(Util::ArrayList<std::shared_ptr<Objects::MassSpringMesh> >& objects, Physics::Collision::Aggregates::TriangleSoupCollisionEventAggregate<Objects::MassSpringMesh>* const collisionData, bool b) {
	
	//if ( b ) {
		
	//}
	//else {
	
		//--------------------------------------------------------------------------
		// Vertex-Triangle Collision Events
		//--------------------------------------------------------------------------
		unsigned int objectVIndex = 0;
		unsigned int otherTriIndex0 = 0, otherTriIndex1 = 0, otherTriIndex2 = 0;
		Eigen::Vector3d vertex, pVertex, triangleV0, triangleV1, triangleV2, pTriangleV0, pTriangleV1, pTriangleV2;
		Eigen::Vector3d displacement, velocity, tri0Velocity, tri1Velocity, tri2Velocity;
		Eigen::Vector3d intPoint, invIntDir, disp, pointDisp, triDisp;

		// NEED AN INDEX MAP SO THAT THE SAME COLLISION RESOLUTION IS NOT APPLIED
		// TO THE SAME POINT TWICE!!

		for ( unsigned int i = 0; i < collisionData->eventpool_VertexFace.size(); i++ ) {
			//----------------------------------------------------------------------
			// Object
			//----------------------------------------------------------------------
			std::shared_ptr<Objects::MassSpringMesh>& object = collisionData->eventpool_VertexFace[i].object;
			objectVIndex = collisionData->eventpool_VertexFace[i].object_vertex_index;

			if ( object == nullptr ) {
				std::cerr << "[MSM_TS_InverseVelocityResponseHandler:performResponse] Error: Cannot resolve collision with nullptr 'object'." << std::endl;
				std::cerr << "  Aborting collision response. " << std::endl;
				return false;
			}

			//----------------------------------------------------------------------
			// Other
			//----------------------------------------------------------------------
			std::shared_ptr<Objects::MassSpringMesh>& other = collisionData->eventpool_VertexFace[i].other;
			otherTriIndex0 = collisionData->eventpool_VertexFace[i].other_triangle_index0;
			otherTriIndex1 = collisionData->eventpool_VertexFace[i].other_triangle_index1;
			otherTriIndex2 = collisionData->eventpool_VertexFace[i].other_triangle_index2;

			if ( other == nullptr ) {
				std::cerr << "[MSM_TS_InverseVelocityResponseHandler:performResponse] Error: Cannot resolve collision with nullptr 'other'." << std::endl;
				std::cerr << "  Aborting collision response. " << std::endl;
				return false;
			}

			//----------------------------------------------------------------------
			// Both are moving
			//----------------------------------------------------------------------
			if ( object->isStatic() == false && other->isStatic() == false )
				Handle_Triangle_Moving_Vertex_Moving(object, other, objectVIndex, otherTriIndex0, otherTriIndex1, otherTriIndex2);

			//----------------------------------------------------------------------
			// Vertex is static, Triangle is moving
			//----------------------------------------------------------------------
			//if ( object->isStatic() == true && other->isStatic() == false )
			//	Handle_Triangle_Moving_Vertex_Static(object, other, objectVIndex, otherTriIndex0, otherTriIndex1, otherTriIndex2);

			//----------------------------------------------------------------------
			// Vertex is moving, Triangle is static
			//----------------------------------------------------------------------
			if ( object->isStatic() == false && other->isStatic() == true ) 
				Handle_Vertex_Moving_Triangle_Static(object, other, objectVIndex, otherTriIndex0, otherTriIndex1, otherTriIndex2);
	
		}

	//}

	//--------------------------------------------------------------------------
		// Edge-Edge Collision Events
		//--------------------------------------------------------------------------
		unsigned int objectEdgeIndex0, objectEdgeIndex1;
		unsigned int otherEdgeIndex0, otherEdgeIndex1;
		for ( unsigned int i = 0; i < collisionData->eventpool_EdgeEdge.size(); i++ ) {
			//----------------------------------------------------------------------
			// Object
			//----------------------------------------------------------------------
			std::shared_ptr<Objects::MassSpringMesh>& object = collisionData->eventpool_EdgeEdge[i].object;
			objectEdgeIndex0 = collisionData->eventpool_EdgeEdge[i].object_edge_index0;
			objectEdgeIndex1 = collisionData->eventpool_EdgeEdge[i].object_edge_index1;

			if ( object == nullptr ) {
				std::cerr << "[MSM_TS_InverseVelocityResponseHandler:performResponse] Error: Cannot resolve collision with nullptr 'object'." << std::endl;
				std::cerr << "  Aborting collision response. " << std::endl;
				return false;
			}

			//----------------------------------------------------------------------
			// Other
			//----------------------------------------------------------------------
			std::shared_ptr<Objects::MassSpringMesh>& other = collisionData->eventpool_EdgeEdge[i].other;
			otherEdgeIndex0 = collisionData->eventpool_EdgeEdge[i].other_edge_index0;
			otherEdgeIndex1 = collisionData->eventpool_EdgeEdge[i].other_edge_index1;

			if ( other == nullptr ) {
				std::cerr << "[MSM_TS_InverseVelocityResponseHandler:performResponse] Error: Cannot resolve collision with nullptr 'other'." << std::endl;
				std::cerr << "  Aborting collision response. " << std::endl;
				return false;
			}

			//double t = collisionData->eventpool_EdgeEdge[i].getLerpValue();
			//Handle_Edge_Edge(object, other, objectEdgeIndex0, objectEdgeIndex1, otherEdgeIndex0, otherEdgeIndex1, t);
		
		}

	return true;
}
